#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/float32_multi_array.hpp"
#include <geometry_msgs/msg/point.hpp>
#include <geometry_msgs/msg/pose.hpp>
#include <geometry_msgs/msg/vector3.hpp>
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
#include <tf2_ros/transform_listener.h>
#include <tf2_ros/buffer.h>
#include <leg1Ang.h>
#include <leg2Ang.h>
#include <leg3Ang.h>
#include <leg4Ang.h>

#include "robot_state_msg/msg/robot_state.hpp"

class RobotControl2 : public rclcpp::Node
{
    private:
        geometry_msgs::msg::Pose body_pose_;
        std::array<geometry_msgs::msg::Point, 4> shoulder_positions_;
        std::array<geometry_msgs::msg::Point, 4> foot_positions_;
        float time;
        //
        // 发布者
        rclcpp::Publisher<std_msgs::msg::Float32MultiArray>::SharedPtr servo_targets_pub_;
        //订阅者
        rclcpp::Subscription<robot_state_msg::msg::RobotState>::SharedPtr robot_state_sub_;
        // 机器人状态回调函数
        void robotStateCallback(const robot_state_msg::msg::RobotState::SharedPtr msg)
        {
            // 更新身体姿态
            body_pose_ = msg->body_pose;
            // 更新足端位置
            for (int i = 0; i < 4; ++i) 
            {
                foot_positions_[i] = msg->foot_positions[i];
            }
            //时长
            time=msg->time;
            // 立即执行计算和发布
            calculateAndPublish();
        }
        // 定时器回调函数
        void calculateAndPublish()
        {
            // 将肩部相对于身体中心的位置转换到世界坐标系
            std::array<geometry_msgs::msg::Point, 4> shoulder_world_positions;
            for (int i = 0; i < 4; i++) {
                shoulder_world_positions[i] = transformPoint(shoulder_positions_[i], body_pose_);
            }
            RCLCPP_INFO(this->get_logger(), "world shoulder1 position: [%f, %f, %f]", shoulder_world_positions[0].x, shoulder_world_positions[0].y, shoulder_world_positions[0].z);
            // 计算四条腿的向量
            std::array<geometry_msgs::msg::Point, 4> leg_vectors;
            for (int i = 0; i < 4; i++) {
                leg_vectors[i].x = foot_positions_[i].x - shoulder_world_positions[i].x;
                leg_vectors[i].y = foot_positions_[i].y - shoulder_world_positions[i].y;
                leg_vectors[i].z = foot_positions_[i].z - shoulder_world_positions[i].z;
            }
            RCLCPP_INFO(this->get_logger(), "leg1 vector: [%f, %f, %f]", leg_vectors[0].x, leg_vectors[0].y, leg_vectors[0].z);
            // 调用 leg1Ang 函数计算腿1的角度
            double L1S1, L1S2, L1S3, L2S1, L2S2, L2S3, L3S1, L3S2, L3S3, L4S1, L4S2, L4S3;
            leg1Ang(leg_vectors[0].x, leg_vectors[0].y, leg_vectors[0].z, &L1S1, &L1S2, &L1S3);
            leg2Ang(leg_vectors[1].x, leg_vectors[1].y, leg_vectors[1].z, &L2S1, &L2S2, &L2S3);
            leg3Ang(leg_vectors[2].x, leg_vectors[2].y, leg_vectors[2].z, &L3S1, &L3S2, &L3S3);
            leg4Ang(leg_vectors[3].x, leg_vectors[3].y, leg_vectors[3].z, &L4S1, &L4S2, &L4S3);
            // 创建 Float32MultiArray 消息
            std_msgs::msg::Float32MultiArray servo_targets_msg;
            servo_targets_msg.data.resize(13);
            servo_targets_msg.data[0]=static_cast<float>(L1S1);
            servo_targets_msg.data[1]=static_cast<float>(L1S2);
            servo_targets_msg.data[2]=static_cast<float>(L1S3);
            servo_targets_msg.data[3]=static_cast<float>(L2S1);
            servo_targets_msg.data[4]=static_cast<float>(L2S2);
            servo_targets_msg.data[5]=static_cast<float>(L2S3);
            servo_targets_msg.data[6]=static_cast<float>(L3S1);
            servo_targets_msg.data[7]=static_cast<float>(L3S2);
            servo_targets_msg.data[8]=static_cast<float>(L3S3);
            servo_targets_msg.data[9]=static_cast<float>(L4S1);
            servo_targets_msg.data[10]=static_cast<float>(L4S2);
            servo_targets_msg.data[11]=static_cast<float>(L4S3);
            servo_targets_msg.data[12]=time;
            // 发布消息
            servo_targets_pub_->publish(servo_targets_msg);
            // 打印日志
            RCLCPP_INFO(this->get_logger(), "Published servo targets: [%f, %f, %f]", L1S1, L1S2, L1S3);
        }
        // 将点从身体坐标系转换到世界坐标系
        geometry_msgs::msg::Point transformPoint(const geometry_msgs::msg::Point& point, const geometry_msgs::msg::Pose& body_pose)
        {
            tf2::Quaternion q(
                body_pose.orientation.x,
                body_pose.orientation.y,
                body_pose.orientation.z,
                body_pose.orientation.w
            );
            tf2::Vector3 p(point.x, point.y, point.z);
            tf2::Transform transform(q, tf2::Vector3(body_pose.position.x, body_pose.position.y, body_pose.position.z));
            tf2::Vector3 p_world = transform * p;

            geometry_msgs::msg::Point result;
            result.x = p_world.x();
            result.y = p_world.y();
            result.z = p_world.z();
            return result;
        }
    public:
        RobotControl2(std::string name) : Node(name)
        {
            RCLCPP_INFO(this->get_logger(), "大家好，我是%s.", name.c_str());
            // 初始化身体姿态
            body_pose_.position.x = 0.0;
            body_pose_.position.y = 0.0;
            body_pose_.position.z = 80.0; // 身体高度为 200
            body_pose_.orientation.x = 0.0;
            body_pose_.orientation.y = 0.0;
            body_pose_.orientation.z = 0.0;
            body_pose_.orientation.w = 1.0; // 无旋转，身体水平
            // 初始化四条腿的足端位置
            foot_positions_[0].x = 172.0+20.0;
            foot_positions_[0].y = 75.0 + 73.5; // 第一条腿
            foot_positions_[0].z = 0.0;

            foot_positions_[1].x = -172.0-20.0;
            foot_positions_[1].y = 75.0 + 73.5; // 第二条腿
            foot_positions_[1].z = 0.0;

            foot_positions_[2].x = 172.0+20.0;
            foot_positions_[2].y = -75.0 - 73.5; // 第三条腿
            foot_positions_[2].z = 0.0;

            foot_positions_[3].x = -172.0-20.0;
            foot_positions_[3].y = -75.0 - 73.5; // 第四条腿
            foot_positions_[3].z = 0.0;
            //初始化肩部相对于身体中心坐标系的位置
            shoulder_positions_[0].x=172.0;
            shoulder_positions_[0].y=75.0;
            shoulder_positions_[0].z=0.0;
            shoulder_positions_[1].x=-172.0;
            shoulder_positions_[1].y=75.0;
            shoulder_positions_[1].z=0.0;
            shoulder_positions_[2].x=172.0;
            shoulder_positions_[2].y=-75.0;
            shoulder_positions_[2].z=0.0;
            shoulder_positions_[3].x=-172.0;
            shoulder_positions_[3].y=-75.0;
            shoulder_positions_[3].z=0.0;
            //
            time=500.0;
            //创建订阅（使用自定义接口）
            robot_state_sub_ = this->create_subscription<robot_state_msg::msg::RobotState>(
                "robot_state_topic", 10,
                std::bind(&RobotControl2::robotStateCallback, this, std::placeholders::_1));
            // 创建发布者，发布 Float32MultiArray 到 /servo_targets 主题
            servo_targets_pub_ = this->create_publisher<std_msgs::msg::Float32MultiArray>(
                "/servo_targets", 10
            );
        }
};

int main(int argc, char **argv)
{
    rclcpp::init(argc, argv);
    /*创建对应节点的共享指针对象*/
    auto node = std::make_shared<RobotControl2>("robot_control2");
    /* 运行节点，并检测退出信号*/
    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}